#include "SeekPoIState.h"
#include "MainGameState.h"
#include "Timer.h"
#include "Search.h"
#include "Random.h"
#include <iostream>
#include <assert.h>

SeekPoIState::SeekPoIState()
{
	m_Target = nullptr;
	m_SelectNewPoI = true;
	m_PointOnPath = 0;
	m_Graph = nullptr;
	m_HasEntered = false;
	m_Timer = 0;
}

SeekPoIState::~SeekPoIState()
{

}

void SeekPoIState::Update(AiAgent* Agent)
{
	float dt = TheTimer::Instance()->GetDeltaTime();
	m_Graph = Agent->ReturnGraph();

	if(m_SelectNewPoI)
	{
		//select new poi
		std::vector<PointOfInterest>* PoIVec = &Agent->ReturnPoIVec();
		int PoISize = PoIVec->size();
		assert(PoISize > 0);

		static int NewNode = 0;
		while(true)
		{
			NewNode = RandomInt(0, PoISize);
			//If the random point isn't the current target, and it's not in our visited vector, then we have our new target. else we choose another random point
			if(&PoIVec->at(NewNode) != m_Target)
			{
				break;
			}
		}

		m_Target = &PoIVec->at(NewNode);
		std::cout << "Agent " << Agent->ReturnId() <<"'s New Target : " << NewNode << "\n";


		//Create a path from closest node to PoI Node
		Search s;
		m_Path = s.BFS(*m_Graph, m_Graph->ReturnIDofClosest(Agent->ReturnPos()), m_Graph->ReturnIDofClosest(m_Target->ReturnPos()));
		Agent->SetPath(m_Path);

		m_PointOnPath = 0;
		if(m_Path.size() > 0)
		{
			m_SelectNewPoI = false;
		}
	}

	else // we have a target and are navigating to it through the graph
	{
		//if we're at the PoI
		float DistanceToTarget = VecLength(Agent->ReturnPos() - m_Target->ReturnPos());
		if(DistanceToTarget <= m_Target->ReturnDistance())
		{
			if(!m_HasEntered)
			{
				//Entered the point of interest for the first time
				Agent->SetVel(Vec3f());
				Agent->SetAcc(Vec3f());

				//React
				m_HasEntered = true;
			}
		}

		else
		{
			//if we've reach the next node on the path, and the path isn't over
			if(m_PointOnPath < m_Path.size()-1)
			{
				float DistanceToPoint = VecLength(Agent->ReturnPos() - m_Graph->GetNode(m_Path[m_PointOnPath]).ReturnPos());
				if(DistanceToPoint <= 1.0f)
				{
					m_PointOnPath++;
					Agent->SetVel(Agent->ReturnVel() * 0.75);
					Agent->SetAcc(Agent->ReturnAcc() * 0.75);
				}
				Agent->SetAcc(Normalised(m_Graph->GetNode(m_Path[m_PointOnPath]).ReturnPos() - Agent->ReturnPos()) * 10);
			}

			else if(m_PointOnPath == m_Path.size()-1)
			{
				Agent->SetVel(Normalised(m_Target->ReturnPos() - Agent->ReturnPos()) * VecLength(m_Target->ReturnPos() - Agent->ReturnPos()));
			}
		}

		if(m_HasEntered)
		{
			m_Timer += dt;
			Agent->SetTextureTag(m_Target->ReturnTexTag());
			if(m_Timer > m_Target->ReturnTime())
			{
				Agent->SetTextureTag("Walking");
				m_SelectNewPoI = true;
				m_Timer = 0;
				m_HasEntered = false;
			}
		}
	}
}